package behaviours;

import misc.Log;
import core.Actuators;
import core.Senses;

public class Turn extends Behaviour {

	private static final float OUTER_TOLERANCE = 0.0073f;
	private static final float INNER_TOLERANCE = 0.0049f;
	private float ot, it;
	private float orientation;
	private float destination = 0.25f;
	private boolean isRelative = false;
	public static final int ABSOLUTE = 0, RELATIVE = 1; 
	
	private static float normalize(float src, float dest){
		float x = (dest-src)*2, sign = Math.signum(x), range=Math.signum(x-sign);
		return x-(range+sign)*range*sign;
	}

	@Override
	protected boolean isFinished() {
		Senses.readCompass();
		orientation = normalize(Senses.getCompass(),destination);
		return Math.abs(orientation) < OUTER_TOLERANCE;
	}

	@Override
	protected void begin() throws InterruptedException{
		Senses.readCompass();
		float x1 = Senses.getCompass();
		Actuators.setMotors(1, -1);
		Actuators.setMotors(0, 0);
		Thread.sleep(120);
		Senses.readCompass();
		float x2 = Senses.getCompass();
		Actuators.setMotors(-1, 1);
		Actuators.setMotors(2, -2);
		Actuators.setMotors(0, 0);
		Thread.sleep(120);
		Senses.readCompass();
		float x3 = Senses.getCompass();
		Actuators.setMotors(-2, 2);
		Actuators.setMotors(0, 0);
		Thread.sleep(120);
		Senses.readCompass();
		float x4 = Senses.getCompass();
		Log.debug("KALI: "+x1+" "+x2+" "+x3+" "+x4+" ");
		ot = x2-x1; it = x3-x2;
		if(isRelative)
			destination = Senses.getCompass()-(destination%360)/360;
		destination %= 1;
	}

	@Override
	protected boolean handleArguments(Object[] arguments) {
		if(arguments.length == 2){
			destination = arguments[1] == null ? 0 : (Float)arguments[1];
			isRelative = true;
			return true;
		}else if(arguments.length == 1){
			destination = arguments[0] == null ? 0 : (Float)arguments[0];
			isRelative = false;
			return true;
		}else{
			Log.error("Invalid Number of Arguments: "+arguments.length);
			return false;
		}
	}

	@Override
	protected void loop() throws InterruptedException{
		int speed = Math.round(orientation*8+Math.signum(orientation)*(Math.abs(orientation) < ot ? 2 : 1));
		Actuators.setMotors(speed,-speed);
		Actuators.setMotors(0, 0);
	}

	@Override
	protected void end() throws InterruptedException{
		Actuators.setMotors(0, 0);
	}

}
